#include "../include/Viewer.hpp"
namespace ORBSLAM
{
    Viewer::Viewer(/* args */) : mb_adjust_scale(false), mf_s(1.0)
    {
    }

    Viewer::~Viewer()
    {
    }

    void Viewer::Set_mapatlas(MapAtlas *p_mapatlas)
    {
        mp_mapatlas = p_mapatlas;
    }

    void Viewer::showTrajectoryandpoints(Map *p_map)
    {
        VecSE3 pose_s;
        // for (size_t i = 0; i < poses_R.size(); i++)
        // {
        //     pose_s.push_back(Sophus::SE3<double, 0>(poses_R[i], 10 * poses_t[i]));
        // }

        vector<KeyFrame *> vp_KF = p_map->Get_all_keyframe();
        const vector<MapPoint *> &vp_mpt = p_map->Get_all_mappoint();

        pangolin::CreateWindowAndBind("Trajectory and points Viewer", 1024, 768);
        glEnable(GL_DEPTH_TEST);
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_CONSTANT_ALPHA);
        pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1, 0));
        pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1, pangolin::Attach::Pix(175), 1, -1024.0 / 768.0).SetHandler(new pangolin::Handler3D(s_cam));
        while (pangolin::ShouldQuit() == false)
        {
            glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //消除颜色缓冲
            glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
            d_cam.Activate(s_cam);

            pangolin::glDrawAxis(3);

            // draw poses
            for (int i = 0; i < vp_KF.size(); i++) //从poses中取位姿
            {
                KeyFrame *p_KF = vp_KF[i];
                Mat Twc = p_KF->Get_Twc().t();
                glPushMatrix();
                // Sophus::Matrix4f m = Tcw.matrix().cast<float>();
                glMultMatrixf(Twc.ptr<GLfloat>(0));

                // const float w = 0.25;
                const float w = 0.16;
                const float h = w * static_cast<float>(0.75);
                const float z = w * static_cast<float>(0.6);
                glColor3f(1, 0, 0);
                glLineWidth(1);
                glBegin(GL_LINES);
                glVertex3f(0, 0, 0);
                glVertex3f(w, h, z);
                glVertex3f(0, 0, 0);
                glVertex3f(w, -h, z);
                glVertex3f(0, 0, 0);
                glVertex3f(-w, -h, z);
                glVertex3f(0, 0, 0);
                glVertex3f(-w, h, z);
                glVertex3f(w, h, z);
                glVertex3f(w, -h, z);
                glVertex3f(-w, h, z);
                glVertex3f(-w, -h, z);
                glVertex3f(-w, h, z);
                glVertex3f(w, h, z);
                glVertex3f(-w, -h, z);
                glVertex3f(w, -h, z);

                glEnd();
                glPopMatrix();
            }
            glLineWidth(2);
            glColor3f(0, 0, 1);
            glBegin(GL_LINES);
            for (size_t i = 0; i < vp_KF.size() - 1; i++)
            {
                Mat t1 = vp_KF[i]->Get_twc();
                Mat t2 = vp_KF[i + 1]->Get_twc();
                glVertex3d(t1.at<float>(0), t1.at<float>(1), t1.at<float>(2));
                glVertex3d(t2.at<float>(0), t2.at<float>(1), t2.at<float>(2));
            }
            glEnd();
            if (true)
            {
                glPointSize(3.0f);
                glColor3f(0, 1, 0);
                glBegin(GL_POINTS);
                for (size_t k = 0; k < vp_mpt.size(); k++)
                {
                    MapPoint *p_mpt = vp_mpt[k];
                    Mat pos = p_mpt->Get_world_pose();
                    glVertex3d(10 * pos.at<float>(0), 10 * pos.at<float>(1), 10 * pos.at<float>(2));
                }
                //点的创建
                glEnd();
            }
            // cv::waitKey(0);
            pangolin::FinishFrame();
        }
        pangolin::DestroyWindow("Trajectory and points Viewer");
        return;
    }

    void Viewer::Run()
    {
        pangolin::CreateWindowAndBind("Trajectory and points Viewer", 1024, 768);
        glEnable(GL_DEPTH_TEST);
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_CONSTANT_ALPHA);
        pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1, 0));
        pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1, pangolin::Attach::Pix(175), 1, -1024.0 / 768.0).SetHandler(new pangolin::Handler3D(s_cam));

        while (!pangolin::ShouldQuit())
        {
            glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //消除颜色缓冲
            glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
            d_cam.Activate(s_cam);
            pangolin::glDrawAxis(3);
            Draw_pose_trajectory_points();
            pangolin::FinishFrame();
        }
        pangolin::DestroyWindow("Trajectory and points Viewer");
        return;
    }

    void Viewer::Draw_pose_trajectory_points()
    {
        mvp_keyframe = mp_mapatlas->Get_current_map()->Get_all_keyframe_ordered();
        mvp_mpt = mp_mapatlas->Get_current_map()->Get_all_mappoint();
        if (mvp_keyframe.empty())
        {
            return;
        }
        if (mvp_mpt.empty())
        {
            return;
        }
        if (!mb_adjust_scale && mvp_keyframe.size() >= 10)
        {
            mb_adjust_scale = true;
            float sum_translation = 0;
            for (int i = 1; i < mvp_keyframe.size(); i++)
            {
                sum_translation += norm(mvp_keyframe[i]->Get_tcw() - mvp_keyframe[i - 1]->Get_tcw());
            }
            sum_translation = sum_translation / (mvp_keyframe.size() - 1);
            mf_s = 0.4/sum_translation;
        }
        for (int i = 0; i < mvp_keyframe.size(); i++) //从poses中取位姿
        {
            KeyFrame *p_KF = mvp_keyframe[i];
            Mat Twc_t = p_KF->Get_Twc().t();
            Mat twc_adjust =  mf_s*Twc_t.colRange(0,3).row(3);
            twc_adjust.copyTo(Twc_t.colRange(0,3).row(3));
            glPushMatrix();
            // Sophus::Matrix4f m = Tcw.matrix().cast<float>();
            glMultMatrixf(Twc_t.ptr<GLfloat>(0));

            // const float w = 0.25;
            const float w = 0.2;
            const float h = w * static_cast<float>(0.75);
            const float z = w * static_cast<float>(0.6);
            glColor3f(1, 0, 0);
            glLineWidth(1.6);
            glBegin(GL_LINES);
            glVertex3f(0, 0, 0);
            glVertex3f(w, h, z);
            glVertex3f(0, 0, 0);
            glVertex3f(w, -h, z);
            glVertex3f(0, 0, 0);
            glVertex3f(-w, -h, z);
            glVertex3f(0, 0, 0);
            glVertex3f(-w, h, z);
            glVertex3f(w, h, z);
            glVertex3f(w, -h, z);
            glVertex3f(-w, h, z);
            glVertex3f(-w, -h, z);
            glVertex3f(-w, h, z);
            glVertex3f(w, h, z);
            glVertex3f(-w, -h, z);
            glVertex3f(w, -h, z);

            glEnd();
            glPopMatrix();
        }
        glLineWidth(2);
        glColor3f(0, 0, 1);
        glBegin(GL_LINES);
        for (size_t i = 0; i < mvp_keyframe.size() - 1; i++)
        {
            // cout<<"keyframe id........................................................................."<<mvp_keyframe[i]->mn_KF_id<<endl;
            Mat t1 = mvp_keyframe[i]->Get_twc()*mf_s;
            Mat t2 = mvp_keyframe[i + 1]->Get_twc()*mf_s;
            glVertex3d(t1.at<float>(0), t1.at<float>(1), t1.at<float>(2));
            glVertex3d(t2.at<float>(0), t2.at<float>(1), t2.at<float>(2));
        }
        glEnd();

        if (true)
        {
            glPointSize(2.0f);
            glColor3f(0, 1, 0);
            glBegin(GL_POINTS);
            for (size_t k = 0; k < mvp_mpt.size(); k++)
            {
                MapPoint *p_mpt = mvp_mpt[k];
                Mat pos = p_mpt->Get_world_pose()*mf_s;
                glVertex3d(10 * pos.at<float>(0), 10 * pos.at<float>(1), 10 * pos.at<float>(2));
            }
            //点的创建
            glEnd();
        }
    }

} // namespace ORBSLAM
